
#include "node/node.h"

#include <chrono>
#include <string>
#include <vector>
#include <cstdlib>

namespace cartographer {

namespace carto = ::cartographer;

Node::Node(const NodeOptions& node_options, const Eigen::Matrix4f& Tvl, std::shared_ptr<Publisher> publisher_ptr) : node_options_(node_options){
    map_builder_interface_ptr_ = std::make_shared<mapping::MapBuilderInterface>(Tvl, node_options.trajectory_id, publisher_ptr);
    publisher_ptr_ = publisher_ptr;
}

// 在析构是执行一次全局优化
Node::~Node() { FinishAllTrajectories(); }

/**
 * @brief 新增一个位姿估计器
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] options 参数配置
 */
void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms

  // // 新生成的轨迹的id 不应该在extrapolators_中
  // CHECK(extrapolators_.count(trajectory_id) == 0);

  // imu_gravity_time_constant在2d, 3d中都是10
  const double gravity_time_constant = 10;

  // 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

/**
 * @brief 新生成一个传感器数据采样器
 * 
 * @param[in] trajectory_id 轨迹id
 * @param[in] options 参数配置
 */
void Node::AddSensorSamplers(const int trajectory_id,
                             const TrajectoryOptions& options) {
  // CHECK(sensor_samplers_.count(trajectory_id) == 0);
  // sensor_samplers_.emplace(
  //     std::piecewise_construct, 
  //     std::forward_as_tuple(trajectory_id),
  //     std::forward_as_tuple(
  //         options.rangefinder_sampling_ratio, 
  //         options.odometry_sampling_ratio,
  //         options.fixed_frame_pose_sampling_ratio, 
  //         options.imu_sampling_ratio,
  //         options.landmarks_sampling_ratio));
}

/**
 * @brief 根据配置文件, 确定所有需要的topic的名字的集合
 *
 * @param[in] options TrajectoryOptions的配置文件
 * @return std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
 */
std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
  /*
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    struct SensorId {
      SensorType type;  // 传感器的种类
      std::string id;   // topic的名字
    };
  */
 
  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
  using SensorType = SensorId::SensorType;
  std::set<SensorId> expected_topics;
  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.

  // 如果只有一个传感器, 那订阅的topic就是topic
  // 如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推
  // for (const std::string& topic :
  //      ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
  //   expected_topics.insert(SensorId{SensorType::RANGE, topic});
  // }
  // for (const std::string& topic : ComputeRepeatedTopicNames(
  //          kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
  //   expected_topics.insert(SensorId{SensorType::RANGE, topic});
  // }
  // for (const std::string& topic :
  //      ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
  //   expected_topics.insert(SensorId{SensorType::RANGE, topic});
  // }
  // // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // // required.
  // // 3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
  // if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
  //     (node_options_.map_builder_options.use_trajectory_builder_2d() &&
  //      options.trajectory_builder_options.trajectory_builder_2d_options()
  //          .use_imu_data())) {
  //   expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
  // }
  // // Odometry is optional.
  // // 里程计可有可无, topic的个数只能有一个
  // if (options.use_odometry) {
  //   expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
  // }
  // // NavSatFix is optional.
  // // gps可有可无, topic的个数只能有一个
  // if (options.use_nav_sat) {
  //   expected_topics.insert(
  //       SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
  // }
  // // Landmark is optional.
  // // Landmark可有可无, topic的个数只能有一个
  // if (options.use_landmarks) {
  //   expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
  // }
  // 返回传感器的topic名字
  return expected_topics;
}

/**
 * @brief 添加一个新的轨迹
 *
 * @param[in] options 轨迹的参数配置
 * @return int 新生成的轨迹的id
 */
int Node::AddTrajectory(const TrajectoryOptions& options) {

  const std::set<mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);

  // 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
  const int trajectory_id = 0; // map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);

  // 新增一个位姿估计器
  AddExtrapolator(trajectory_id, options);

  // 新生成一个传感器数据采样器
  AddSensorSamplers(trajectory_id, options);

  // 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
}

/**
 * @brief 订阅话题与注册回调函数
 * 
 * @param[in] options 配置参数
 * @param[in] trajectory_id 轨迹id  
 */
void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  // for (const std::string& topic :
  //      ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
  // }

}

// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  std::lock_guard<std::mutex> lock(mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  // CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

std::tuple<sensor::PointCloudWithIntensities, common::Time>
Node::ToPointCloudWithIntensities(const LaserScan& msg) {
    CHECK_GE(msg.range_min, 0.f);
    CHECK_GE(msg.range_max, msg.range_min);
    if (msg.angle_increment > 0.f) {
      CHECK_GT(msg.angle_max, msg.angle_min);
    } else {
      CHECK_GT(msg.angle_min, msg.angle_max);
    }

    sensor::PointCloudWithIntensities point_cloud;
    float angle = msg.angle_min;
    for (size_t i = 0; i < msg.ranges.size(); ++i) {
      // c++11: 使用auto可以适应不同的数据类型
      const auto& echoes = msg.ranges[i];
      if (HasEcho(echoes)) {

        const float first_echo = GetFirstEcho(echoes);
        // 满足范围才进行使用
        if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
          const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
          const sensor::TimedRangefinderPoint point{
              rotation * (first_echo * Eigen::Vector3f::UnitX()), // position
              i * (float)msg.time_increment};                            // time
          // 保存点云坐标与时间信息
          point_cloud.points.push_back(point);

          // LOG(INFO) << " x y pose : " << msg.x_coords[i] << ", " << msg.y_coords[i]  << std::endl;
          // LOG(INFO) << " position : " << point.position.transpose() << std::endl;
          
          // 如果存在强度信息
          if (msg.intensities.size() > 0) {
            // CHECK_EQ(msg.intensities.size(), msg.ranges.size());
            // 使用auto可以适应不同的数据类型
            const auto& echo_intensities = msg.intensities[i];
            // CHECK(HasEcho(echo_intensities));
            point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
          } else {
            point_cloud.intensities.push_back(0.f);
          }
        }
      } else {
          // if (msg.ranges[i] < msg.range_min || msg.ranges[i] > msg.range_max) {
          //   continue;
          // }

          // double real_angle = msg.angle_min + i * msg.angle_increment;
          // double x = msg.ranges[i] * std::cos(real_angle);
          // double y = msg.ranges[i] * std::sin(real_angle);

          // //std::cout << "real_angle : " << real_angle << std::endl;

          // if (real_angle < msg.angle_min || real_angle > msg.angle_max) {
          //     continue;
          // }

          // const sensor::TimedRangefinderPoint point{
          //     Eigen::Vector3f(x, y, 0), // position
          //     i * msg.time_increment};                            // time
          // // 保存点云坐标与时间信息
          // point_cloud.points.push_back(point);
        
          // // 如果存在强度信息
          // if (msg.intensities.size() > 0) {
          //   CHECK_EQ(msg.intensities.size(), msg.ranges.size());
          //   // 使用auto可以适应不同的数据类型
          //   const auto& echo_intensities = msg.intensities[i];
          //   CHECK(HasEcho(echo_intensities));
          //   point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
          // } else {
          //   point_cloud.intensities.push_back(0.f);
          // }
      }
      angle += msg.angle_increment;
    }
  
    common::Time timestamp = common::FromSecond(msg.scan_time);

    // LOG(INFO) << " msg.scan_time : " << std::to_string(msg.scan_time*1e9) << std::endl;

    // LOG(INFO) << " timestamp : " << timestamp << std::endl;

    if (!point_cloud.points.empty()) {
      const double duration = point_cloud.points.back().time;
      // 以点云最后一个点的时间为点云的时间戳
      timestamp += common::FromSeconds(duration);

      // 让点云的时间变成相对值, 最后一个点的时间为0
      for (auto& point : point_cloud.points) {
        point.time -= duration;
      }
    }
    return std::make_tuple(point_cloud, timestamp);
}

// 结束所有处于活动状态的轨迹
void Node::FinishAllTrajectories() {

}

void Node::HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                                 const sensor::OdometryData& odometry_data) {

    // LOG(INFO) << " sensor_id : " << sensor_id << std::endl;
    
    // extrapolators_.at(trajectory_id).AddOdometryData(odometry_data);
    map_builder_interface_ptr_->HandleOdometry(sensor_id, odometry_data);
}

void Node::HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                            const sensor::ImuData& imu_data) {

    // LOG(INFO) << " extrapolators_ : " << extrapolators_.size() << std::endl;

    // extrapolators_.at(trajectory_id).AddImuData(imu_data);
    map_builder_interface_ptr_->HandleImu(sensor_id, imu_data);
}

// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int& trajectory_id, const std::string& sensor_id,
                                  const LaserScan& laser_scan) {
    std::lock_guard<std::mutex> lock(mutex_);

    sensor::PointCloudWithIntensities point_cloud;
    common::Time time;
    std::tie(point_cloud, time) = ToPointCloudWithIntensities(laser_scan);
    map_builder_interface_ptr_->HandleLaserScan(sensor_id, time, point_cloud);
}

}  // namespace cartographer_ros